#include "StdAfx.h"
#include "AxServerModelGotoPoseLimit.h"

AxServerModelGotoPoseLimit::AxServerModelGotoPoseLimit(ArServerBase *server, ArRobot *robot, ArPose home /*= ArPose(0, 0, 0)*/, ArRetFunctor<ArPose> *getHomePoseCB /*= NULL*/):ArServerMode(robot, server, "AxGoto"), myRatioDriveGroup(robot),
	myServerGotoPoseCB(this, &AxServerModelGotoPoseLimit::serverGotoPose)
{
	myServer = server;
	myRobot = robot;
	myGotoAction=new ArActionGoto("goto",ArPose(0,0,0),100.0,200.0,100.0,10.0);
	addModeData("axGotoPose", 
		"sends the robot to a given x, y and optional heading", 
		&myServerGotoPoseCB, 
		"byte4: x byte4: y (optional) byte4: th", "none", "Navigation",
		"RETURN_NONE");
	 myRatioDriveGroup.addAction(myGotoAction,50);

	 limiterAction=new ArActionLimiterForwards("speed limiter near", 300, 600, 200);
	 myRatioDriveGroup.addAction(limiterAction, 95);
	 limiterFarAction=new ArActionLimiterForwards("speed limiter far", 300, 1100, 400);
	 myRatioDriveGroup.addAction(limiterFarAction, 90);
	 limiterBackwardsAction=new ArActionLimiterBackwards("speed limiter Backwards", 300, 1000, 200);
	 myRatioDriveGroup.addAction(limiterBackwardsAction, 85);

	 myLimiterForward = new ArActionDeceleratingLimiter(
		 "DeceleratingLimiterForward", ArActionDeceleratingLimiter::FORWARDS);
	 myRatioDriveGroup.addAction(myLimiterForward, 40);

	 myLimiterBackward = new ArActionDeceleratingLimiter(
		 "DeceleratingLimiterBackward",ArActionDeceleratingLimiter::BACKWARDS);
	 myRatioDriveGroup.addAction(myLimiterBackward, 39);

	 myLimiterLateralLeft = NULL;
	 myLimiterLateralRight = NULL;
	 if (myRobot->hasLatVel())
	 {
		 myLimiterLateralLeft = new ArActionDeceleratingLimiter(
			 "DeceleratingLimiterLateralLeft",  ArActionDeceleratingLimiter::LATERAL_LEFT);
		 myRatioDriveGroup.addAction(myLimiterLateralLeft, 38);
		 myLimiterLateralRight = new ArActionDeceleratingLimiter(
			 "DeceleratingLimiterLateralRight", ArActionDeceleratingLimiter::LATERAL_RIGHT);
		 myRatioDriveGroup.addAction(myLimiterLateralRight, 37);
	 }
}


AxServerModelGotoPoseLimit::~AxServerModelGotoPoseLimit(void)
{
}

void AxServerModelGotoPoseLimit::serverGotoPose(ArServerClient * /*client*/, ArNetPacket *packet)
{
	ArPose pose;
	bool useHeading = false;

	pose.setX(packet->bufToByte4());
	pose.setY(packet->bufToByte4());
	if (packet->getDataLength() > packet->getDataReadLength())
	{
		useHeading = true;
		pose.setTh(packet->bufToByte4());
	}
	bool first = true;
	ArTime start;
	start.setToNow();
	/*while (Aria::getRunning()) 
	{*/
		if (first || myGotoAction->haveAchievedGoal())
		{
			first = false;
			myRobot->lock();
			ArLog::log(ArLog::Normal, "Going to point");
			//gotoPose(pose, useHeading);
			myGotoAction->setGoal(pose);
			activate();
			myRatioDriveGroup.activateExclusive();
			myRobot->unlock();
			ArLog::log(ArLog::Normal, "Going to next goal at %.0f %.0f", myGotoAction->getGoal().getX(), myGotoAction->getGoal().getY());
		}
	//}
	long duration=30000;
	if(start.mSecSince() >= duration) 
	{
		ArLog::log(ArLog::Normal, "%d seconds have elapsed. Cancelling current goal, waiting 3 seconds, and exiting.", duration/1000);
		myGotoAction->cancelGoal();
		myRobot->unlock();
		ArUtil::sleep(3000);
	}
	
}

void AxServerModelGotoPoseLimit::activate(void)
{
	ArLog::log(ArLog::Normal, "AxServerModeGotoPose::activate");
	if (!baseActivate())
	{
		return;
	}

	myDone = false;
	setActivityTimeToNow();
}

void AxServerModelGotoPoseLimit::userTask(void)
{
	if (!myDone)
	{
		setActivityTimeToNow();

	}
}

void AxServerModelGotoPoseLimit::deactivate(void)
{
	myRatioDriveGroup.deactivate();
	baseDeactivate();
}
